#include "adjds371.h"
#include "LEDs.h"
#include "light_sensor.h"
#include "system.h"

#include <stdio.h>

void measure_light(void);
unsigned int get_color(unsigned char);

int redGain = 0x08;
int greenGain = 0x0a;
int blueGain = 0x03;

void increase_red_capacitance(void) {
	char capText[4];

	redGain++;
	if (redGain > 0xf)
		redGain = 0;

	adjds371_write_byte(CAP_RED, redGain);

	sprintf(capText,"R:x%X",redGain);
	set_led_text(capText);
}

void increase_green_capacitance(void) {
	char capText[4];

	greenGain++;
	if (greenGain > 0xf)
		greenGain = 0;

	adjds371_write_byte(CAP_GREEN, greenGain);

	sprintf(capText,"G:x%X",greenGain);
	set_led_text(capText);

}

void increase_blue_capacitance(void) {
	char capText[4];

	blueGain++;
	if (blueGain > 0xf)
		blueGain = 0;

	adjds371_write_byte(CAP_BLUE, blueGain);
	
	sprintf(capText,"B:x%X",blueGain);
	set_led_text(capText);
}


void light_sensor_initialize(void) {
	int red = 0;
	int green = 0;
	int blue = 0;
	int multiplier = 2000;

	adjds371_initialize();
	
	// Write initial sensor gain settings (higher gain results in less saturation)
	adjds371_write_byte(CAP_RED, redGain);
	adjds371_write_byte(CAP_GREEN, greenGain);
	adjds371_write_byte(CAP_BLUE, blueGain);

	adjds371_write_byte(INT_RED_LOW, 0xC0);
	adjds371_write_byte(INT_RED_HIGH, 0x09);
	adjds371_write_byte(INT_GREEN_LOW, 0xC0);
	adjds371_write_byte(INT_GREEN_HIGH, 0x09);
	adjds371_write_byte(INT_BLUE_LOW, 0xC0);
	adjds371_write_byte(INT_BLUE_HIGH, 0x09);
}

void read_sensor(void) {
	int red = 0;
	int green = 0;
	int blue = 0;
	int i =0;
	int clear_gain = 0;
	int color_gain = 0;
	float red_multiplier, green_multiplier, blue_multiplier;
	int red_val, green_val, blue_val;
	unsigned long rgb_val;

	// Take NUM_SAMPLES samples and average them together.
	for (i=0; i<NUM_SAMPLES; i++) {
		measure_light();
		red += get_color(DATA_RED);
		green += get_color(DATA_GREEN);
		blue += get_color(DATA_BLUE);
	}
	red /= NUM_SAMPLES;
	green /= NUM_SAMPLES;
	blue /= NUM_SAMPLES;

	red_val = (int)(((float)(red/1023.0))*255);
	green_val = (int)(((float)(green/1023.0))*255);
	blue_val = (int)(((float)(blue/1023.0))*255);

	rgb_val = (0xFF000000)+(red_val<<16)+(green_val<<8)+(blue_val);

	printf("%lu\n",rgb_val);
}

void measure_light(void) {
	unsigned char data_byte = 0xFF;

	adjds371_write_byte(SENCTL, SENCTL_INIT);
	while (1) {
		data_byte = adjds371_read_byte(SENCTL);
		if (data_byte == SENCTL_READY) {
			break;
		}
	}
}

unsigned int get_color(unsigned char color_byte) {
	return adjds371_read_byte(color_byte) + (adjds371_read_byte(color_byte+1)<<8);
}